﻿/*
  motion_control.c - high level interface for issuing motion commands
  Part of Grbl

  Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
  Copyright (c) 2009-2011 Simen Svale Skogsrud

  Grbl is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  Grbl is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "grbl.h"

// 在绝对毫米坐标下执行线性运动。进给速度以毫米/秒为单位给出除非invert_feed_rate设置为true。进给速度的意思是运动
// 应该在(1分钟)/进给速度时间内完成。注意：这是grbl规划器的一个重要通道。所有的线性运动，包括圆弧线段，都必须通过这个程序
// 传递给规划器。mc_line和plan_buffer_line的分离主要是为了将非规划器类型函数从规划器中分离出来，并让回差补偿或
// 预设循环集成变得简单直接。
// target 绝对目标位置
void mc_line(float *target, plan_line_data_t *pl_data)
{
  //如果限位使能， 就检查target值是否超出了轴能到达的最远位置， 如果超出了就告警限位错误， 并复位系统
  if (bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)) {
    // 注意：阻止手动状态。手动控制是一个特殊情况，软件限位会被独立地处理。
    if (sys.state != STATE_JOG) { limits_soft_check(target); }
  }

  // 如果处于g代码检查模式，则被规划器块阻止运动。软限位仍然工作。
  if (sys.state == STATE_CHECK_MODE) { return; }

  // 注意：反冲补偿可能会安装在这。它需要方向信息去追踪，当当前运动线段之前插入一条反冲运动线段时。并且它需要

  // 检查block_buffer是否满， 如果满就执行尝试打开线段插补执行使能开关。 如果系统配置里开启了autocycle功能， 就可以自动开始执行线段插补， 
  // 否则一直循环等待环形队列有空闲

  do {
    protocol_execute_realtime(); // 检查任何运行时的命令，如暂停或复位。
    if (sys.abort) { return; } // Bail, if system abort.

    // 如果缓冲区满了，就开始执行并循环等待缓冲区有空位
    if ( plan_check_full_buffer() ) { protocol_auto_cycle_start(); } // Auto-cycle start when buffer is full.
    else { break; }
  } while (1);

  //把当前线段的的信息添加到block_buffer队列里， 这个函数里包含了前瞻算法
  if (plan_buffer_line(target, pl_data) == PLAN_EMPTY_BLOCK) {
    if (bit_istrue(settings.flags, BITFLAG_LASER_MODE)) {
      // 如果传递了一个重合的位置，则正确设置主轴状态。仅在M3激光模式下强制缓冲区同步。
      if (pl_data->condition & PL_COND_FLAG_SPINDLE_CW) {
        spindle_sync(PL_COND_FLAG_SPINDLE_CW, pl_data->spindle_speed);
      }
    }
  }
}

// mc_arc 函数用于在偏移模式下执行圆弧运动. 使用很多小线段进行拟合计算。arc_tolerance 定义了拟合时的精度
//
// target：目标位置（xyz）
// pl_data：计划线数据
// position：当前位置
// offset：偏移量
// radius：圆弧半径
// axis_0、axis_1：圆弧所在的平面。是用来指定在哪个坐标平面内进行圆弧运动（可以是 XY、XZ 或 YZ 平面）。
// axis_linear：圆弧线性轴。比如，如果圆弧运动在 XY 平面上，而 axis_linear 是 Z 轴，则表示这是一个在 XY 平面上的圆弧，同时沿着 Z 轴进行线性移动的螺旋运动。
// is_clockwise_arc：是否顺时针弧度
void mc_arc(float *target, plan_line_data_t *pl_data, float *position, float *offset, float radius,
  uint8_t axis_0, uint8_t axis_1, uint8_t axis_linear, uint8_t is_clockwise_arc)
{
  float center_axis0 = position[axis_0] + offset[axis_0];
  float center_axis1 = position[axis_1] + offset[axis_1];
  float r_axis0 = -offset[axis_0];  // Radius vector from center to current location
  float r_axis1 = -offset[axis_1];
  float rt_axis0 = target[axis_0] - center_axis0;
  float rt_axis1 = target[axis_1] - center_axis1;

  // CCW angle between position and target from circle center. Only one atan2() trig computation required.
  float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
  if (is_clockwise_arc) { // Correct atan2 output per direction
    if (angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel -= 2*M_PI; }
  } else {
    if (angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel += 2*M_PI; }
  }

  // 使用内切多边形线段近似圆弧，最大误差为 2*settings.arc_tolerance 变量值
  // 如果需要不同的近似方式（例如线段中点在圆弧上），则更改 mm_per_arc_segment 计算方式即可

  // 计算圆弧的近似线段数量
  uint16_t segments = floor(fabs(0.5*angular_travel*radius)/
                          sqrt(settings.arc_tolerance*(2*radius - settings.arc_tolerance)) );

  if (segments) {
    float theta_per_segment = angular_travel/segments;  // 每个线段对应的角增量
    float linear_per_segment = (target[axis_linear] - position[axis_linear])/segments; // 每个线段在 axis_linear 方向上的线性增量（螺旋线）

    /* 通过变换矩阵进行向量旋转：r 是原始向量，r_T 是旋转后的向量，
       phi 是旋转角度。解决方案由 Jens Geisler 提出。
           r_T = [cos(phi) -sin(phi);
                  sin(phi)  cos(phi)] * r ;

       在圆弧生成过程中，圆的中心为旋转轴，半径向量由圆心到初始位置定义。
       每个线段通过连续的向量旋转形成。单精度值在极少数情况下可能会累积超过工具精度的误差。
       因此，实现了精确圆弧路径校正。这种方法避免了过多计算昂贵的三角函数操作 [sin(),cos(),tan()] 的问题，
       这些操作每次计算可能需要 100-200 微秒。

       可以使用小角度近似来进一步减少计算开销。对于大多数，如果不是全部的 CNC 应用，
       三阶近似（二阶 sin() 误差太大）是适用的。需要注意的是，当 theta_per_segment 大于
       ~0.25 弧度（14 度）并且多次连续使用近似而没有校正时，这种近似会开始累积数值漂移误差。
       这种情况极不可能发生，因为线段长度和 theta_per_segment 会根据 arc_tolerance 设置自动生成并缩放。
       只有非常大的 arc_tolerance 设置，对于 CNC 应用来说是不现实的，才可能导致这种数值漂移误差。
       但是，最好将 N_ARC_CORRECTION 设置在一个较低值（如 ~4）到一个较高值（如 ~20）之间，
       以避免三角函数操作的同时保持圆弧生成的准确性。

       这种近似方法还允许 mc_arc 函数立即将一个线段插入到规划器中，
       而无需初始计算 cos() 或 sin() 的开销。等到需要对弧进行校正时，
       规划器应该已经追上了初始 mc_arc 开销造成的延迟。
       当存在连续的弧运动时，这一点非常重要。
    */

    // Computes: cos_T = 1 - theta_per_segment^2/2, sin_T = theta_per_segment - theta_per_segment^3/6) in ~52usec
    // 使用小角度近似计算旋转矩阵
    float cos_T = 2.0 - theta_per_segment*theta_per_segment;
    float sin_T = theta_per_segment*0.16666667*(cos_T + 4.0);
    cos_T *= 0.5;

    float sin_Ti;
    float cos_Ti;
    float r_axisi;
    uint16_t i;
    uint8_t count = 0;

    // 处理每个近似的线段
    for (i = 1; i<segments; i++) { // Increment (segments-1).

      if (count < N_ARC_CORRECTION) {
        // Apply vector rotation matrix. ~40 usec
        r_axisi = r_axis0*sin_T + r_axis1*cos_T;
        r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
        r_axis1 = r_axisi;
        count++;
      } else {
        // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments. ~375 usec
        // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
        cos_Ti = cos(i*theta_per_segment);
        sin_Ti = sin(i*theta_per_segment);
        r_axis0 = -offset[axis_0]*cos_Ti + offset[axis_1]*sin_Ti;
        r_axis1 = -offset[axis_0]*sin_Ti - offset[axis_1]*cos_Ti;
        count = 0;
      }

      // Update arc_target location
      position[axis_0] = center_axis0 + r_axis0;
      position[axis_1] = center_axis1 + r_axis1;
      position[axis_linear] += linear_per_segment;

      // 将目标位置插入到线段规划器队列中
      mc_line(position, pl_data);

      // Bail mid-circle on system abort. Runtime command check already performed by mc_line.
      if (sys.abort) { return; }
    }
  }

  // 确保最后一个线段到达目标位置
  mc_line(target, pl_data);
}


// Execute dwell in seconds.
void mc_dwell(float seconds)
{
  if (sys.state == STATE_CHECK_MODE) { return; }
  protocol_buffer_synchronize();
  delay_sec(seconds, DELAY_MODE_DWELL);
}


// Perform homing cycle to locate and set machine zero. Only '$H' executes this command.
// NOTE: There should be no motions in the buffer and Grbl must be in an idle state before
// executing the homing cycle. This prevents incorrect buffered plans after homing.
void mc_homing_cycle(uint8_t cycle_mask)
{
  // Check and abort homing cycle, if hard limits are already enabled. Helps prevent problems
  // with machines with limits wired on both ends of travel to one limit pin.
  // TODO: Move the pin-specific LIMIT_PIN call to limits.c as a function.
  #ifdef LIMITS_TWO_SWITCHES_ON_AXES
    if (limits_get_state()) {
      mc_reset(); // Issue system reset and ensure spindle and coolant are shutdown.
      system_set_exec_alarm(EXEC_ALARM_HARD_LIMIT);
      return;
    }
  #endif

  limits_disable(); // Disable hard limits pin change register for cycle duration

  // -------------------------------------------------------------------------------------
  // Perform homing routine. NOTE: Special motion case. Only system reset works.
  
  #ifdef HOMING_SINGLE_AXIS_COMMANDS
    if (cycle_mask) { limits_go_home(cycle_mask); } // Perform homing cycle based on mask.
    else
  #endif
  {
    // Search to engage all axes limit switches at faster homing seek rate.
    limits_go_home(HOMING_CYCLE_0);  // Homing cycle 0
    #ifdef HOMING_CYCLE_1
      limits_go_home(HOMING_CYCLE_1);  // Homing cycle 1
    #endif
    #ifdef HOMING_CYCLE_2
      limits_go_home(HOMING_CYCLE_2);  // Homing cycle 2
    #endif
  }

  protocol_execute_realtime(); // Check for reset and set system abort.
  if (sys.abort) { return; } // Did not complete. Alarm state set by mc_alarm.

  // Homing cycle complete! Setup system for normal operation.
  // -------------------------------------------------------------------------------------

  // Sync gcode parser and planner positions to homed position.
  gc_sync_position();
  plan_sync_position();

  // If hard limits feature enabled, re-enable hard limits pin change register after homing cycle.
  limits_init();
}

// 使用探针检测工具长度检测，需要探针开关
// 探针失败时，程序会停止并置于警告状态
uint8_t mc_probe_cycle(float *target, plan_line_data_t *pl_data, uint8_t parser_flags)
{
  // TODO: Need to update this cycle so it obeys a non-auto cycle start.
  if (sys.state == STATE_CHECK_MODE) { return(GC_PROBE_CHECK_MODE); }

  // Finish all queued commands and empty planner buffer before starting probe cycle.
  protocol_buffer_synchronize();
  if (sys.abort) { return(GC_PROBE_ABORT); } // Return if system reset has been issued.

  // Initialize probing control variables
  uint8_t is_probe_away = bit_istrue(parser_flags,GC_PARSER_PROBE_IS_AWAY);
  uint8_t is_no_error = bit_istrue(parser_flags,GC_PARSER_PROBE_IS_NO_ERROR);
  sys.probe_succeeded = false; // Re-initialize probe history before beginning cycle.
  probe_configure_invert_mask(is_probe_away);

  // After syncing, check if probe is already triggered. If so, halt and issue alarm.
  // NOTE: This probe initialization error applies to all probing cycles.
  if ( probe_get_state() ) { // Check probe pin state.
    system_set_exec_alarm(EXEC_ALARM_PROBE_FAIL_INITIAL);
    protocol_execute_realtime();
    probe_configure_invert_mask(false); // Re-initialize invert mask before returning.
    return(GC_PROBE_FAIL_INIT); // Nothing else to do but bail.
  }

  // Setup and queue probing motion. Auto cycle-start should not start the cycle.
  mc_line(target, pl_data);

  // Activate the probing state monitor in the stepper module.
  sys_probe_state = PROBE_ACTIVE;

  // Perform probing cycle. Wait here until probe is triggered or motion completes.
  system_set_exec_state_flag(EXEC_CYCLE_START);
  do {
    protocol_execute_realtime();
    if (sys.abort) { return(GC_PROBE_ABORT); } // Check for system abort
  } while (sys.state != STATE_IDLE);

  // Probing cycle complete!

  // Set state variables and error out, if the probe failed and cycle with error is enabled.
  if (sys_probe_state == PROBE_ACTIVE) {
    if (is_no_error) { memcpy(sys_probe_position, sys_position, sizeof(sys_position)); }
    else { system_set_exec_alarm(EXEC_ALARM_PROBE_FAIL_CONTACT); }
  } else {
    sys.probe_succeeded = true; // Indicate to system the probing cycle completed successfully.
  }
  sys_probe_state = PROBE_OFF; // Ensure probe state monitor is disabled.
  probe_configure_invert_mask(false); // Re-initialize invert mask.
  protocol_execute_realtime();   // Check and execute run-time commands

  // Reset the stepper and planner buffers to remove the remainder of the probe motion.
  st_reset(); // Reset step segment buffer.
  plan_reset(); // Reset planner buffer. Zero planner positions. Ensure probing motion is cleared.
  plan_sync_position(); // Sync planner position to current machine position.

  #ifdef MESSAGE_PROBE_COORDINATES
    // All done! Output the probe position as message.
    report_probe_parameters();
  #endif

  if (sys.probe_succeeded) { return(GC_PROBE_FOUND); } // Successful probe cycle.
  else { return(GC_PROBE_FAIL_END); } // Failed to trigger probe within travel. With or without error.
}


// Plans and executes the single special motion case for parking. Independent of main planner buffer.
// NOTE: Uses the always free planner ring buffer head to store motion parameters for execution.
#ifdef PARKING_ENABLE
  void mc_parking_motion(float *parking_target, plan_line_data_t *pl_data)
  {
    if (sys.abort) { return; } // Block during abort.

    uint8_t plan_status = plan_buffer_line(parking_target, pl_data);

    if (plan_status) {
      bit_true(sys.step_control, STEP_CONTROL_EXECUTE_SYS_MOTION);
      bit_false(sys.step_control, STEP_CONTROL_END_MOTION); // Allow parking motion to execute, if feed hold is active.
      st_parking_setup_buffer(); // Setup step segment buffer for special parking motion case
      st_prep_buffer();
      st_wake_up();
      do {
        protocol_exec_rt_system();
        if (sys.abort) { return; }
      } while (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION);
      st_parking_restore_buffer(); // Restore step segment buffer to normal run state.
    } else {
      bit_false(sys.step_control, STEP_CONTROL_EXECUTE_SYS_MOTION);
      protocol_exec_rt_system();
    }

  }
#endif


#ifdef ENABLE_PARKING_OVERRIDE_CONTROL
  void mc_override_ctrl_update(uint8_t override_state)
  {
    // Finish all queued commands before altering override control state
    protocol_buffer_synchronize();
    if (sys.abort) { return; }
    sys.override_ctrl = override_state;
  }
#endif


// Method to ready the system to reset by setting the realtime reset command and killing any
// active processes in the system. This also checks if a system reset is issued while Grbl
// is in a motion state. If so, kills the steppers and sets the system alarm to flag position
// lost, since there was an abrupt uncontrolled deceleration. Called at an interrupt level by
// realtime abort command and hard limits. So, keep to a minimum.
void mc_reset()
{
  // 只有这个函数可以设置系统重置，帮助阻止多次终止调用
  // Only this function can set the system reset. Helps prevent multiple kill calls.
  if (bit_isfalse(sys_rt_exec_state, EXEC_RESET)) {
    system_set_exec_state_flag(EXEC_RESET);

    // Kill spindle and coolant.
    spindle_stop();
    coolant_stop();

    // Kill steppers only if in any motion state, i.e. cycle, actively holding, or homing.
    // NOTE: If steppers are kept enabled via the step idle delay setting, this also keeps
    // the steppers enabled by avoiding the go_idle call altogether, unless the motion state is
    // violated, by which, all bets are off.
    if ((sys.state & (STATE_CYCLE | STATE_HOMING | STATE_JOG)) ||
    		(sys.step_control & (STEP_CONTROL_EXECUTE_HOLD | STEP_CONTROL_EXECUTE_SYS_MOTION))) {
      if (sys.state == STATE_HOMING) { 
        if (!sys_rt_exec_alarm) {system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_RESET); }
      } else { system_set_exec_alarm(EXEC_ALARM_ABORT_CYCLE); }
      st_go_idle(); // Force kill steppers. Position has likely been lost.
    }
  }
}
